#pragma config(Sensor, dgtl1,  EncoderRight,        sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderLeft,         sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  AButton,             sensorTouch)
#pragma config(Motor,  port2,           RightDrive,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           LeftDrive,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm1,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           RightTread,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           LeftArm2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           LeftTread,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Encoder.c"                    //Autonomous Encoder control
#include "PIDController.h"              //Autonomous PID Arm controller
#define MAX_ARM 110
#define MAX_TREAD 127

int KP = 1;
int KI = 0;
int KD = 0;
bool armPID = false;

controller PID;

//set Values
int armGround = SensorValue[potArm];                       
int lowScore = armGround// + value still to be tested;
int medScore = armGround// + value still to be tested;
int highScore = armGround// + value still to be tested; 


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
    encoderConfigure(4,2,90,127,(14.375/12.0),(4.0/12.0));
    init(PID,potArm,port4);
    setSetpoint(PID, highScore);
    setPIDs(PID, KP, KI, KD);
    enable(PID);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
void setCollectorSpeed(int value)
{
  motor[RightTread] = motor[LeftTread] = value;
}

task autonomous()
{
  pre_auton();
  while(true)
  {
    if(vexRT[AButton] == 1)
    {
      setArmSpeed(calcPID(PID));
      encoderDriveStraight(44.0/12.0);
      setCollectorSpeed(MAX_TREAD);
      wait10Msec(400);
      setCollectorSpeed(0);
      setSetPoint(PID, armGround);
      enable(PID);
      encoderDriveStraight(-44.0/12.0);
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////


void tankDrive()
{
  motor[RightDrive] = vexRT[Ch2];
  motor[LeftDrive] = vexRT[Ch3];
}

void armDrive()
{
  if(vexRT[Btn5U] == 1)
  {
    motor[RightArm] = MAX_ARM;
    motor[LeftArm1] = MAX_ARM;
    motor[LeftArm2] = MAX_ARM;
  }
  else if(vexRT[Btn5D] == 1)
  {
    motor[LeftArm1] = -MAX_ARM;
    motor[LeftArm2] = -MAX_ARM;
    motor[RightArm] = -MAX_ARM;

  }
  else
  {
    motor[LeftArm1] = 0;
    motor[LeftArm2] = 0;
    motor[RightArm] = 0;
  }
}

void TreadDrive()
{
  if(vexRT[Btn8D] + vexRT[Btn8U] + vexRT[Btn8L] + vexRT[Btn8R] == 0)
  {
    motor[RightTread] = motor[LeftTread] = (vexRT[Btn6D] - vexRT[Btn6U])*MAX_TREAD;
  }
  else
  {
    motor[RightTread] = (vexRT[Btn8U] -vexRT[Btn8R])*MAX_TREAD;
    motor[LeftTread] = (vexRT[Btn8L] -vexRT[Btn8D])*MAX_TREAD;
  }
}

void PIDArmControl()
{

    if((vexRT[Btn6D] - vexRT[Btn6U])!= 0)
    {  
      setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * maxArm);
      disable(PID);  
    }  
    else
    {  
      setArmSpeed(calcPID(PID));  
    }    
    if(vexRT[Btn7U] == 1)
    {  
      setSetpoint(PID, highScore);
      enable(PID);  
    }  
    else if (vexRT[Btn7D] == 1)
    {  
      setSetpoint(PID, lowScore);
      enable(PID);  
    }  
    else if (vexRT[Btn7L] == 1)
    {  
      setSetpoint(PID, midScore);
      enable(PID);
    }  
    else if (vexRT[Btn7R] == 1)
    {  
      disable(PID);  
    }
}

task usercontrol()
{
  while(true) 
  {
    tankDrive();
    TreadDrive();
    PIDArmControl(); 
  }
}
